/********************************************************
 *
 * FILE NAME:   notchfilter.cpp
 *
 * DESCRIPTION:
 *  Module with notch filter
 *
 ********************************************************/

/********************************************************/
/*              Includes                                */
/********************************************************/
#include "notchfilter.h"

#include <QDebug>

/********************************************************/
/*              Defines                                 */
/********************************************************/

/********************************************************/
/*              Macros                                  */
/********************************************************/

/********************************************************/
/*              Typedefs                                */
/********************************************************/

/********************************************************/
/*              Local Functions Declarations (LOCAL)    */
/********************************************************/

/********************************************************/
/*              Global Variables (GLOBAL/IMPORT)        */
/********************************************************/

/********************************************************/
/*              Local Module Variables (MODULE)         */
/********************************************************/

/********************************************************/
/*              Local File Variables (LOCAL)            */
/********************************************************/

/********************************************************/
/*              Functions Definitions (LOCAL/GLOBAL)    */
/********************************************************/

NotchFilter::NotchFilter(Filter *parent) : Filter(parent){}


void NotchFilter::init(int burstLength)
{
    a = new sample_t[3];
    aLength = 3;

    b = new sample_t[3];
    bLength = 3;

    input = new ISignal(3, burstLength);
    output = new FilterOSignal(this, 3);
}

void NotchFilter::setCoefficients(double Wo, double BW)
{
    Wo *= 2 * M_PI;
    BW *= 2 * M_PI;

    sample_t Gb = pow(10, (-abs(10 * log10(0.5)) / 20));
    sample_t beta = (sqrt(1 - pow(Gb, 2)) / Gb) * tan(BW / 2);
    sample_t gain = 1 / (1 + beta);

    a[0] = 1;
    a[1] = -2 * gain * cos(Wo);
    a[2] = 2 * gain - 1;

    b[0] = gain;
    b[1] = -2 * gain * cos(Wo);
    b[2] = gain;
}

NotchFilter::~NotchFilter()
{
    if(a != nullptr)
    {
        delete[] a;
    }
    if(b != nullptr)
    {
        delete[] b;
    }
}
